/* 
 * File:   robibounder.hpp
 * Author: posypkin
 *
 * Created on July 14, 2012, 12:26 PM
 *
 * The Interval based bounder for Robotic Manipulator
 */

#ifndef ROBIBOUNDER_HPP
#define	ROBIBOUNDER_HPP

#include <util/common/interval.hpp>
#include <problems/pareto/multicov/bounder.hpp>

template <typename FT> class RobIntervalBounder : public Bounder < FT > {
public:

    RobIntervalBounder(int nsec, FT* sections, FT *signs) {
        mNSec = nsec;
        mSections = sections;
        mSigns = signs;
        mDim = 2;
    }

    /**
     * Retrieve the lower bounds
     *
     * @param box the box to evaluate the bound
     * @param crit the place to store the lower bounds for criteria (NULL means no bound is needed)
     * @param cons the place to store the lower bounds for constraints (NULL means no bound is needed)
     */
    void getLowerBound(const Box<FT>& box, FT* crit, FT* cons) const {
        FT al = 0, au = 0;
        FT phil = 0, phiu = 0;
        FT cril[mDim], criu[mDim];
        for(int i = 0; i < mDim; i ++) {
            cril[i] = 0;
            criu[i] = 0;
        }
        for (int i = 0; i < mNSec; i++) {
            FT bu, bl;
            BNBInterval<FT>::sum(phil, phiu, box.mA[i], box.mB[i], &phil, &phiu);
            BNBInterval<FT>::cos(phil, phiu, &bl, &bu);
            cril[0] += mSections[i] * bl;
            criu[0] += mSections[i] * bu;
            BNBInterval<FT>::sin(phil, phiu, &bl, &bu);
            cril[1] += mSections[i] * bl;
            criu[1] += mSections[i] * bu;
        }
        for (int i = 0; i < mDim; i++) {
            if (mSigns[i] > 0) {
                crit[i] = cril[i];
            } else {
                crit[i] = -criu[i];
            }
        }
    }


private:
    int mNSec;
    int mDim;
    FT* mSections;
    FT* mSigns;

};




#endif	/* ROBIBOUNDER_HPP */

